#!/usr/bin/python
import roslib; roslib.load_manifest('DelphinROSv2')
import rospy
import serial
import re
import time
from DelphinROSv2.msg import compass
from DelphinROSv2.msg import status
global serialPort


################################################################
def setUpSerial():
    global serialPort
    serialPort = serial.Serial(port='/dev/usboceanserver', baudrate='115200', timeout=0.01) # may need to change timeout if having issues!
    serialPort.bytesize = serial.EIGHTBITS
    serialPort.stopbits = serial.STOPBITS_ONE
    serialPort.parity = serial.PARITY_NONE
    print "Initialised OceanServer serial."
    print serialPort.portstr
    print serialPort.isOpen()
    return serialPort.isOpen()
    
    
################################################################
def listenForData():
    global serialPort

    while not rospy.is_shutdown():    
        try:
            while serialPort.inWaiting() > 0 and serialPort.read(1) == '$':     #while there is data to be read - read the line...
            
                data = serialPort.readline(size = None, eol = '\n')
                p = re.compile('[-]*\d+.\d+')                                   #Regular expression to extract data from message
                data = p.findall(data)
 
                try:
                    heading     = float(data[0])
                    heading     = heading-90
                    heading     = heading%360

                    # horrible bodge +ve bow up
                    pitch       = float(data[2])-180
                    if pitch <-180:
                        pitch=pitch%360
                        
                    roll        =-float(data[1])                    
                    temperature = float(data[3])           
                    depth       = (float(data[4])*41.27)-0.35                   #Convert ADC value to depth value
                    
                    m           = float(data[5])
                    mx          = float(data[7])
                    my          =-float(data[6])
                    mz          =-float(data[8])
                    
                    a           = float(data[9])
                    ax          = float(data[11])
                    ay          =-float(data[10])
                    az          =-float(data[12])
                    
                    print '*******'
                    print 'heading %f' %(heading)
                    print 'pitch %f' %(pitch)
                    print 'roll %f' %(roll)
                    print 'temperature %f' %(temperature)
                    print 'depth %f' %(depth)
                    print 'm %f' %(m)
                    print 'mx %f' %(mx)
                    print 'my %f' %(my)
                    print 'mz %f' %(mz)
                    print 'a %f' %(a)
                    print 'ax %f' %(ax)
                    print 'ay %f' %(ay)
                    print 'az %f' %(az)
     
                    #Publish data to compass_out
                    pub.publish(heading=heading,pitch=pitch,roll=roll,temperature=temperature,depth=depth,m=m,mx=mx,my=my,mz=mz,a=a,ax=ax,ay=ay,az=az)
                    
                except ValueError: 
                    print 'not a float'
        except:
            print 'read error'

def validDataCheck():
    while not serialPort.read(1) == '$':
        pass
    data = serialPort.readline(size = None, eol = '\n')		#Read in line of data
    p = re.compile('[-]*\d+.\d+')                                   #Regular expression to extract data from message
    data = p.findall(data)
    print data
    if len(data) == 13:
        return True
    else:
        return False

################################################################
def shutdown():
    serialPort.flushInput()
    serialPort.flushOutput()
    pubStatus.publish(nodeID = 5, status = False)
    serialPort.close()

################################################################        
#     INITIALISE     ###########################################
################################################################

if __name__ == '__main__':
    time.sleep(3) #Allow System to come Online    
    rospy.init_node('OceanServer_compass')
    
    global pub
    global serialPort
    pub = rospy.Publisher('compass_out', compass)   
    pubStatus = rospy.Publisher('status', status)
    
    rospy.on_shutdown(shutdown)  
    
    port_status = setUpSerial()
    str = "OceanServer port status = %s. Port = %s" %(port_status, serialPort.portstr)
    rospy.loginfo(str)
    time.sleep(0.3)
    string_status=validDataCheck()
    
    if (port_status and string_status) == True:    
        pubStatus.publish(nodeID = 5, status = True)
        rospy.loginfo("OceanServer online")
    else:
        pubStatus.publish(nodeID = 5, status = False)
      
    
    listenForData()   #Main loop for receiving data
    
    
